双臂协同示教与搬运使用说明

[TOC]

1. 功能概述

本功能面向两台机械臂共同夹持箱体、工件等刚性对象的场景。两台机械臂由同一个 Planner管理，分别设为 MASTER 和 SLAVE，规划器使用一条协同对象轨迹同时生成主、从臂关节轨迹。

推荐将应用分为两个阶段：

1. 示教阶段：使用双臂协同 speedL 示教共同夹持对象；到达需要的协同路点后停止，记录两台实际机器人的实际反馈关节角。
2. 搬运阶段：把每组示教关节角填入同一个 PathPoint 的 joint_m 和 joint_s，设置双臂协同逆解策略，通过 tpAddPositionLine()（下文简称协同 moveL）依次执行。记录的双臂关节角用于恢复每个示教点的协同对象位姿和对应构型。

speedL 只用于示教，其速度轨迹不是最终搬运路径。最终复现的是停稳后记录的协同路点。

总体流程如下：

建立双臂场景与单 Planner（MASTER + SLAVE）
                    ↓
夹持协同对象，以实际关节状态初始化双臂并进入 IDEL
                    ↓
通过协同 speedL 示教 → 减速停稳 → 记录 PathPoint
                    ↓
重复示教，得到按顺序排列的协同路点
                    ↓
双臂回到首个路点并重新建立固定变换
                    ↓
从第二个路点开始添加协同 moveL 路径并执行搬运

2. 支持范围与约束

2.1 规划器组织方式

必须创建一个同时包含主、从机械臂的 Planner：

auto planner_h = scene->rlCreateRobotPlanner(
    {{master_model_h, master_state_h, PlannerRole::MASTER},
     {slave_model_h,  slave_state_h,  PlannerRole::SLAVE}},
    "dual_arm_transport_planner");

当前支持的协同类型为：

IKStrategy::ROBOTIC_ARM_IK_STRATEGY

不能使用两个独立 Planner 替代本流程；两个独立规划器只能做同启或同步调度，不会自动维持同一搬运对象与两个工具之间的刚性关系。

2.2 坐标系与工具

• 两个 Model 必须在同一 Scene 中，并通过 mdlSetBasePoseInWorld() 填写现场实际安装位姿。
• 主、从臂必须分别配置工具类型、TCP 和负载：主臂使用 type/tool/payload，从臂使用 type_s/tool_s/payload_s。从臂的三个可选字段必须同时填写。
• 两臂使用同一 workpiece，该字段表示协同对象/路点参考系在世界坐标系下的位姿。
• 当前双臂协同运动不支持远端工具，type 和 type_s 应均设为 0。

调用 Task::tskSetTaskState(TaskState::IDEL) 将规划器设置为 IDEL 状态时，Plan 会从主、从 State 中读取当前关节角和各自的工具信息，计算两个 TCP 的世界位姿并建立双臂固定变换：协同对象原点取两个 TCP 世界位置的中点，姿态沿用主臂 TCP 姿态。后续 speedL 和 moveL 都维持初始时协同对象到两个 TCP 的固定变换。

必须先完成夹持，再用两台机器人的实际关节反馈初始化 State 并进入 IDEL。夹持位置、TCP、基座安装位姿变更后，必须重新初始化，不能沿用之前的固定变换。

2.3 安全前提

• 首次运行应降低速度和加速度，在无负载或模拟环境中验证。
• 示教和执行时都需实时监测两臂关节限位、速度、力矩、碰撞及夹持状态。
• 单个离散示教点可达不代表相邻点之间的直线必然可达；每次 tpAddPositionLine() 的返回值都必须检查。
• 算法输出不替代控制器安全功能和现场急停回路。

3. 示教阶段：使用协同 speedL

3.1 初始化双臂状态

初始化顺序如下，完整代码见第 5 节：

1. 按 2.2 节分别配置主、从 State 的 ToolWorkpiece。
2. 通过 rsSetJointMaximumSpeed() 和 rsSetJointMaximumAcceleration() 设置两臂关节安全约束，Plan 会在规划时从 State 读取这些值。
3. 通过 rsInitiateRobotState() 分别写入主、从臂当前状态和工具信息。
4. 设置 Task 的笛卡尔约束后，调用 tskSetTaskState(TaskState::IDEL) 初始化规划起点及双臂固定变换。

3.2 添加 speedL

MoveProperty speed;
speed.move_mode = MoveMode::VELOCITY;
speed.tarV = {0.0, 0.02, 0.0, 0.0, 0.0, 0.0};
speed.maxV = {0.05, 0.05, 0.05, 0.5, 0.5, 0.5};
speed.maxA = {0.2, 0.2, 0.2, 2.0, 2.0, 2.0};
speed.maxJ = {1.6, 1.6, 1.6, 16.0, 16.0, 16.0};

int ret = planner->tpAddVelocityLine(1, DescribeSpace::CARTESIAN, speed, IKStrategy::ROBOTIC_ARM_IK_STRATEGY);
if (ret < 0) {
    // 不得进入执行环，记录 ret 并进入安全态。
    return ret;
}

tarV/maxV/maxA/maxJ 都是 6 维协同对象笛卡尔量：

[vx, vy, vz, wx, wy, wz]

其中线速度单位为 m/s，角速度单位为 rad/s。tarV 的正负号决定示教方向。该速度描述的是协同对象，不是某一台机械臂的 TCP 速度。

speedL 必须使用 CARTESIAN 空间和 ROBOTIC_ARM_IK_STRATEGY，并在包含 SLAVE 且已完成初始化的 Planner 上调用。

3.3 执行与停止

控制周期内应依次执行：

1. 读取主、从臂实时反馈，分别调用 rsUpdateJointState() 更新 State；
2. 调用 Task::tskUpdateCycle() 获取本周期主、从臂轨迹点，只有返回 E_PLN_EXEC_EMPTY 时才表示本周期没有有效轨迹点；
3. 按 TrajectoryPoint::id 找到对应的 State、Controller 和 JointCommand，调用 rsUpdateReferenceTrajectory(true, point) 写入参考轨迹；
4. 分别调用两臂 Controller 的 fcCalJointCommand() 更新两个 JointCommand；
5. 将主、从臂控制指令在同一控制周期下发到对应机器人。

不应依赖 tskUpdateCycle() 输出 vector 的固定顺序。

结束示教操作或到达目标位置后，调用：

ret = task->tskStop(StopType::STOP, 1.0, 0);

应继续执行 tskUpdateCycle() 输出的减速轨迹，直到返回 E_PLN_EXEC_EMPTY (40003)。E_PLN_FINISHED (40002) 仍包含当前路径段的最后有效点，不应跳过。确认两台机械臂都已停稳后，保留 tskUpdateCycle() 返回的最后一组有效轨迹点：该组轨迹点包含主、从臂关节角信息以及当前协同路点位姿信息，上层可直接用于记录示教路点。E_PLN_EXEC_EMPTY 对应的空周期不会更新轨迹点。

如需示教多个点，完成一次停止并记录后，先用当前实测关节状态重新初始化主、从 State，再设置 TaskState::IDEL，然后发送下一条协同 speedL。

3.4 夹持力控说明

双臂共同夹持刚性对象时，实机安装、TCP 标定和控制跟踪误差可能在两臂之间产生内力。必要时应由上层控制系统在示教和搬运阶段配置合适的力控/柔顺控制，并持续监测夹持力、关节力矩和对象滑移。

上述控制链在普通运动控制和力控中相同；差别是 Controller 的控制模式及其参数。speedL 只生成协同运动参考轨迹，不代替力控策略；具体力控参数需根据机型、夹具和搬运对象调试。

4. 搬运阶段：使用协同 moveL

4.1 使用现有示教路点

搬运阶段由上层提供已记录的 PathPoint 示教路点。每个路点必须填写 joint_m、joint_s、完整的主从工具工件信息，并设置 type = JOINT 和 ik_strategy = ROBOTIC_ARM_IK_STRATEGY。

不要把 PathProperty::describe_space 改为 JOINT，否则执行的是双臂联合 moveJ，而不是协同 moveL。

4.2 设置路径和运动参数

PathProperty path;
path.describe_space = DescribeSpace::CARTESIAN;
path.cur_prop.type = CurveType::LINE;
path.blend_radius = {0.0, 0.0};

MoveProperty move;
move.move_mode = MoveMode::POSITION;
move.maxV = {0.05, 0.5};   // [最大线速度, 最大角速度]
move.maxA = {0.2, 2.0};    // [最大线加速度, 最大角加速度]
move.maxJ = {1.6, 16.0};   // [最大线加加速度, 最大角加加速度]

moveL 的 maxV/maxA/maxJ 为 2 维平移/旋转模长约束，与 speedL 的 6 维分量约束不同。

首次调试建议使用 blend_radius = {0.0, 0.0}，确认整条路径可达且夹持稳定后再调整交融。

4.3 执行顺序

搬运前将两臂置于第一个示教点并按 3.1 节初始化；随后从第二个示教点开始添加路径，完成后封闭路径并执行。具体接口调用见第 5 节。

5. 完整 Demo

下面给出一个由 main() 包住的顺序式 Demo。代码分为公共初始化、speedL 示教和 moveL 搬运三段，不额外封装运行时结构。关节反馈变量由上层实时更新，生成的两套 JointCommand 由上层负责下发。

示教和搬运的周期循环都直接展开，并严格执行：

State → Task → ReferenceTrajectory → Controller → 同周期下发

其中两个 Controller 和两套 JointCommand 在公共初始化中创建，随后由两个阶段共同使用。两套命令均计算成功后，由上层在同一个控制周期分别下发到主、从机械臂。

#include <aral/robot_scene.hpp>
#include <vector>

using namespace ARAL::interface;

int main()
{
    constexpr double cycle = 0.005;
    int ret = E_PLN_OK;

    // ======================== 1. 公共初始化 ========================
    auto scene = static_cast<Scene*>(CreateARALScene("dual_arm_transport"));
    if (!scene) return -1;

    const ModelHandle master_model_h = scene->rlCreateRobotModel(
        "master_arm", "/path/to/master/model/");
    const ModelHandle slave_model_h = scene->rlCreateRobotModel(
        "slave_arm", "/path/to/slave/model/");
    if (master_model_h < 0 || slave_model_h < 0) return -1;

    auto master_model = scene->rlGetRobotModel(master_model_h);
    auto slave_model = scene->rlGetRobotModel(slave_model_h);
    if (!master_model || !slave_model) return -1;

    // 必须替换为现场实测的世界坐标系安装位姿。
    ret = master_model->mdlSetBasePoseInWorld(
        {0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
    if (ret < 0) return ret;
    ret = slave_model->mdlSetBasePoseInWorld(
        {0.5, 0.0, 0.0, 0.0, 0.0, 0.0});
    if (ret < 0) return ret;

    const StateHandle master_state_h = scene->rlCreateRobotState(
        master_model_h, "master_state");
    const StateHandle slave_state_h = scene->rlCreateRobotState(
        slave_model_h, "slave_state");
    auto master_state = scene->rlGetRobotState(master_state_h);
    auto slave_state = scene->rlGetRobotState(slave_state_h);
    if (!master_state || !slave_state) return -1;

    const RLJntArray master_max_qd = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
    const RLJntArray master_max_qdd = {6.0, 6.0, 6.0, 6.0, 6.0, 6.0};
    const RLJntArray slave_max_qd = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
    const RLJntArray slave_max_qdd = {6.0, 6.0, 6.0, 6.0, 6.0, 6.0};
    if ((ret = master_state->rsSetJointMaximumSpeed(master_max_qd)) < 0)
        return ret;
    if ((ret = master_state->rsSetJointMaximumAcceleration(master_max_qdd)) < 0)
        return ret;
    if ((ret = slave_state->rsSetJointMaximumSpeed(slave_max_qd)) < 0)
        return ret;
    if ((ret = slave_state->rsSetJointMaximumAcceleration(slave_max_qdd)) < 0)
        return ret;

    ToolWorkpiece master_tw;
    master_tw.type = 0;
    master_tw.tool = {0.0, 0.0, 0.18, 0.0, 0.0, 0.0};
    master_tw.workpiece = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    ToolWorkpiece slave_tw;
    slave_tw.type = 0;
    slave_tw.tool = {0.0, 0.0, 0.12, 0.0, 0.0, 0.0};
    slave_tw.workpiece = master_tw.workpiece;

    ToolWorkpiece coordinated_tw = master_tw;
    coordinated_tw.type_s = slave_tw.type;
    coordinated_tw.tool_s = slave_tw.tool;
    coordinated_tw.payload_s = slave_tw.payload;

    // 公共初始化中创建两个 Controller，并分别绑定主、从 State。
    auto master_controller = scene->rlCreateRobotController(
        master_state_h, "master_controller");
    auto slave_controller = scene->rlCreateRobotController(
        slave_state_h, "slave_controller");
    if (!master_controller || !slave_controller) return -1;

    ret = master_controller->mcInitiateController(ControlType::MOTION);
    if (ret < 0) return ret;
    ret = slave_controller->mcInitiateController(ControlType::MOTION);
    if (ret < 0) return ret;
    ret = master_controller->mcSetControlPeriod(cycle);
    if (ret < 0) return ret;
    ret = slave_controller->mcSetControlPeriod(cycle);
    if (ret < 0) return ret;

    const PlannerHandle planner_h = scene->rlCreateRobotPlanner(
        {{master_model_h, master_state_h, PlannerRole::MASTER},
         {slave_model_h,  slave_state_h,  PlannerRole::SLAVE}},
        "dual_arm_transport_planner");
    auto planner = scene->rlGetRobotPlanner(planner_h);
    auto task = scene->rlCreateRobotTask(
        {planner_h}, "dual_arm_transport_task");
    if (!planner || !task) return -1;

    ret = task->tskSetCycle(cycle);
    if (ret < 0) return ret;
    ret = task->tskSetCartesianVelocityAccelerationLimits(
        {0.2, 0.5}, {4.0, 16.0}, 0);
    if (ret < 0) return ret;

    // 两套 JointCommand 与两个 Controller 一一对应；示教和搬运都复用它们。
    JointCommand master_cmd;
    JointCommand slave_cmd;
    std::vector<PathPoint> taught_points;

    // 以下主、从关节反馈变量由上层使用实际机器人数据实时更新。
    RLJntArray master_q, master_qd, master_qdd;
    RLJntArray master_torque, master_temperature, master_friction;
    RLJntArray slave_q, slave_qd, slave_qdd;
    RLJntArray slave_torque, slave_temperature, slave_friction;

    // ======================== 2. speedL 协同示教 ========================
    {
        // 本次示教前，两臂应已经夹持并停稳。用同一批实时反馈重新建立起点。
        ret = master_state->rsInitiateRobotState(
            master_q, master_qd, master_qdd, master_tw);
        if (ret < 0) return ret;
        ret = slave_state->rsInitiateRobotState(
            slave_q, slave_qd, slave_qdd, slave_tw);
        if (ret < 0) return ret;

        master_cmd.q = master_q;
        master_cmd.qd = master_qd;
        master_cmd.qdd = master_qdd;
        master_cmd.torq = master_torque;
        slave_cmd.q = slave_q;
        slave_cmd.qd = slave_qd;
        slave_cmd.qdd = slave_qdd;
        slave_cmd.torq = slave_torque;

        // 两个 State 均初始化完成后才进入 IDEL，Plan 在这里建立固定变换。
        ret = task->tskSetTaskState(TaskState::IDEL);
        if (ret < 0) return ret;

        MoveProperty speed;
        speed.move_mode = MoveMode::VELOCITY;
        // 实际应用可由示教按钮实时选择方向；这里以 +Y 为例。
        speed.tarV = {0.0, 0.02, 0.0, 0.0, 0.0, 0.0};
        speed.maxV = {0.05, 0.05, 0.05, 0.5, 0.5, 0.5};
        speed.maxA = {0.2, 0.2, 0.2, 2.0, 2.0, 2.0};
        speed.maxJ = {1.6, 1.6, 1.6, 16.0, 16.0, 16.0};

        ret = planner->tpAddVelocityLine(
            1, DescribeSpace::CARTESIAN, speed,
            IKStrategy::ROBOTIC_ARM_IK_STRATEGY);
        if (ret < 0) return ret;

        bool stop_sent = false;
        bool stop_requested = false;  // 上层满足示教停止条件时置为 true。
        bool have_last_valid_points = false;
        TrajectoryPoint last_master_point;
        TrajectoryPoint last_slave_point;

        for (;;) {
            // 示教按钮松开或上层条件满足时只触发一次 STOP。
            // tskStop() 之后不能退出循环，后续减速轨迹仍需完整执行。
            if (!stop_sent && stop_requested) {
                ret = task->tskStop(StopType::STOP, 1.0, 0);
                if (ret < 0) return ret;
                stop_sent = true;
            }

            // State：先读取并更新两臂本周期实际反馈。
            ret = master_state->rsUpdateJointState(
                master_q, master_qd, master_qdd, master_torque,
                master_temperature, master_friction);
            if (ret < 0) return ret;
            ret = slave_state->rsUpdateJointState(
                slave_q, slave_qd, slave_qdd, slave_torque,
                slave_temperature, slave_friction);
            if (ret < 0) return ret;

            // Task：两个 State 都更新后，再取本周期主、从轨迹点。
            std::vector<TrajectoryPoint> points;
            PlannerHandle failed_planner = -1;
            const int plan_ret = task->tskUpdateCycle(
                cycle, points, failed_planner);

            if (plan_ret == E_PLN_EXEC_EMPTY) {
                // STOP 后到这里才表示减速轨迹已全部执行完。
                if (!stop_sent) return -1;
                break;
            }
            const bool valid_status =
                plan_ret == E_PLN_OK || plan_ret == E_PLN_START ||
                plan_ret == E_PLN_FINISHED;
            if (!valid_status) {
                task->tskStop(StopType::EMERGENCY, 1.0, 0);
                return plan_ret;
            }

            // 按 ModelHandle 路由，不依赖 points 的顺序。
            const TrajectoryPoint* master_point = nullptr;
            const TrajectoryPoint* slave_point = nullptr;
            for (const auto& point : points) {
                if (point.id == master_model_h && !master_point)
                    master_point = &point;
                else if (point.id == slave_model_h && !slave_point)
                    slave_point = &point;
                else {
                    task->tskStop(StopType::EMERGENCY, 1.0, 0);
                    return -1;
                }
            }
            if (!master_point || !slave_point) {
                task->tskStop(StopType::EMERGENCY, 1.0, 0);
                return -1;
            }

            // ReferenceTrajectory：分别写入两个 State。
            ret = master_state->rsUpdateReferenceTrajectory(
                true, *master_point);
            if (ret < 0) return ret;
            ret = slave_state->rsUpdateReferenceTrajectory(
                true, *slave_point);
            if (ret < 0) return ret;

            // Controller：分别更新两套 JointCommand，期间不提前下发任何一臂。
            JointCommand next_master_cmd = master_cmd;
            JointCommand next_slave_cmd = slave_cmd;
            ret = master_controller->fcCalJointCommand(next_master_cmd);
            if (ret < 0) return ret;
            ret = slave_controller->fcCalJointCommand(next_slave_cmd);
            if (ret < 0) return ret;

            // next_master_cmd 和 next_slave_cmd 用于在同一控制周期
            // 分别向主、从机械臂下发关节位置、速度、加速度和力矩指令。
            master_cmd = next_master_cmd;
            slave_cmd = next_slave_cmd;

            // E_PLN_FINISHED 仍是有效点，也必须在完成控制和下发后保存。
            last_master_point = *master_point;
            last_slave_point = *slave_point;
            have_last_valid_points = true;
        }

        if (!have_last_valid_points) return -1;
        // 上层确认两臂已停稳且夹持状态正常后记录示教点。

        // 从最后一组有效主、从轨迹点生成一个现有 PathPoint。
        // Task 输出的是两个单臂点，因此从臂关节角取 slave_point.point.joint_m。
        PathPoint taught_point = last_master_point.point;
        taught_point.id = static_cast<double>(taught_points.size());
        taught_point.type = DescribeSpace::JOINT;
        taught_point.joint_m = last_master_point.point.joint_m;
        taught_point.joint_s = last_slave_point.point.joint_m;
        taught_point.t_w = coordinated_tw;
        taught_point.ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY;
        taught_points.push_back(taught_point);
    }

    if (taught_points.size() < 2) return -1;

    // ================= 3. 双臂同步 moveJ 到运动起点 =================
    ret = master_state->rsInitiateRobotState(
        master_q, master_qd, master_qdd, master_tw);
    if (ret < 0) return ret;
    ret = slave_state->rsInitiateRobotState(
        slave_q, slave_qd, slave_qdd, slave_tw);
    if (ret < 0) return ret;

    master_cmd.q = master_q;
    master_cmd.qd = master_qd;
    master_cmd.qdd = master_qdd;
    master_cmd.torq = master_torque;
    slave_cmd.q = slave_q;
    slave_cmd.qd = slave_qd;
    slave_cmd.qdd = slave_qdd;
    slave_cmd.torq = slave_torque;

    ret = task->tskSetTaskState(TaskState::IDEL);
    if (ret < 0) return ret;

    PathPoint movej_target = taught_points.front();
    movej_target.id = 0.0;
    movej_target.type = DescribeSpace::JOINT;
    movej_target.t_w = coordinated_tw;
    movej_target.ik_strategy = IKStrategy::NONE;

    PathProperty movej_path;
    movej_path.describe_space = DescribeSpace::JOINT;
    movej_path.cur_prop.type = CurveType::LINE;
    movej_path.blend_radius = {0.0, 0.0};

    // 约束顺序为主臂六个关节在前、从臂六个关节在后。
    MoveProperty movej;
    movej.move_mode = MoveMode::POSITION;
    movej.maxV = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
    movej.maxA = {3.0, 3.0, 3.0, 3.0, 3.0, 3.0,
                  3.0, 3.0, 3.0, 3.0, 3.0, 3.0};
    movej.maxJ = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0,
                  30.0, 30.0, 30.0, 30.0, 30.0, 30.0};

    ret = planner->tpAddPositionLine(movej_target, movej_path, movej);
    if (ret < 0) return ret;

    for (;;) {
        ret = master_state->rsUpdateJointState(
            master_q, master_qd, master_qdd, master_torque,
            master_temperature, master_friction);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateJointState(
            slave_q, slave_qd, slave_qdd, slave_torque,
            slave_temperature, slave_friction);
        if (ret < 0) return ret;

        std::vector<TrajectoryPoint> points;
        PlannerHandle failed_planner = -1;
        const int plan_ret = task->tskUpdateCycle(
            cycle, points, failed_planner);
        if (plan_ret == E_PLN_EXEC_EMPTY)
            break;

        const bool valid_status =
            plan_ret == E_PLN_OK || plan_ret == E_PLN_START ||
            plan_ret == E_PLN_FINISHED;
        if (!valid_status) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return plan_ret;
        }

        const TrajectoryPoint* master_point = nullptr;
        const TrajectoryPoint* slave_point = nullptr;
        for (const auto& point : points) {
            if (point.id == master_model_h && !master_point)
                master_point = &point;
            else if (point.id == slave_model_h && !slave_point)
                slave_point = &point;
            else {
                task->tskStop(StopType::EMERGENCY, 1.0, 0);
                return -1;
            }
        }
        if (!master_point || !slave_point) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return -1;
        }

        ret = master_state->rsUpdateReferenceTrajectory(true, *master_point);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateReferenceTrajectory(true, *slave_point);
        if (ret < 0) return ret;

        JointCommand next_master_cmd = master_cmd;
        JointCommand next_slave_cmd = slave_cmd;
        ret = master_controller->fcCalJointCommand(next_master_cmd);
        if (ret < 0) return ret;
        ret = slave_controller->fcCalJointCommand(next_slave_cmd);
        if (ret < 0) return ret;

        // next_master_cmd 和 next_slave_cmd 用于在同一控制周期
        // 分别向主、从机械臂下发关节位置、速度、加速度和力矩指令。
        master_cmd = next_master_cmd;
        slave_cmd = next_slave_cmd;
    }

    // 上层确认双臂 moveJ 已执行完成且两臂停稳。

    // 回到首点后必须再次读取实测反馈，不能用保存值冒充当前状态。
    // ======================== 4. moveL 协同搬运 ========================
    ret = master_state->rsInitiateRobotState(
        master_q, master_qd, master_qdd, master_tw);
    if (ret < 0) return ret;
    ret = slave_state->rsInitiateRobotState(
        slave_q, slave_qd, slave_qdd, slave_tw);
    if (ret < 0) return ret;

    master_cmd.q = master_q;
    master_cmd.qd = master_qd;
    master_cmd.qdd = master_qdd;
    master_cmd.torq = master_torque;
    slave_cmd.q = slave_q;
    slave_cmd.qd = slave_qd;
    slave_cmd.qdd = slave_qdd;
    slave_cmd.torq = slave_torque;

    ret = task->tskSetTaskState(TaskState::IDEL);
    if (ret < 0) return ret;

    PathProperty path;
    path.describe_space = DescribeSpace::CARTESIAN;
    path.cur_prop.type = CurveType::LINE;
    path.blend_radius = {0.0, 0.0};

    MoveProperty move;
    move.move_mode = MoveMode::POSITION;
    move.maxV = {0.05, 0.5};
    move.maxA = {0.2, 2.0};
    move.maxJ = {1.6, 16.0};

    // 首点已用于初始化规划起点，从第二个示教点开始添加协同 moveL。
    for (std::size_t i = 1; i < taught_points.size(); ++i) {
        PathPoint target = taught_points[i];
        target.id = static_cast<double>(i);
        target.type = DescribeSpace::JOINT;
        target.t_w = coordinated_tw;
        target.ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY;

        ret = planner->tpAddPositionLine(target, path, move);
        if (ret < 0) return ret;
    }

    for (;;) {
        // State：搬运同样先更新两个 State 的实际反馈。
        ret = master_state->rsUpdateJointState(
            master_q, master_qd, master_qdd, master_torque,
            master_temperature, master_friction);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateJointState(
            slave_q, slave_qd, slave_qdd, slave_torque,
            slave_temperature, slave_friction);
        if (ret < 0) return ret;

        // Task：两个 State 更新完成后取点。
        std::vector<TrajectoryPoint> points;
        PlannerHandle failed_planner = -1;
        const int plan_ret = task->tskUpdateCycle(
            cycle, points, failed_planner);
        if (plan_ret == E_PLN_EXEC_EMPTY)
            break;

        const bool valid_status =
            plan_ret == E_PLN_OK || plan_ret == E_PLN_START ||
            plan_ret == E_PLN_FINISHED;
        if (!valid_status) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return plan_ret;
        }

        const TrajectoryPoint* master_point = nullptr;
        const TrajectoryPoint* slave_point = nullptr;
        for (const auto& point : points) {
            if (point.id == master_model_h && !master_point)
                master_point = &point;
            else if (point.id == slave_model_h && !slave_point)
                slave_point = &point;
            else {
                task->tskStop(StopType::EMERGENCY, 1.0, 0);
                return -1;
            }
        }
        if (!master_point || !slave_point) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return -1;
        }

        // ReferenceTrajectory：分别写入两个 State。
        ret = master_state->rsUpdateReferenceTrajectory(true, *master_point);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateReferenceTrajectory(true, *slave_point);
        if (ret < 0) return ret;

        // Controller：搬运也使用同一对 Controller 和两套 JointCommand。
        JointCommand next_master_cmd = master_cmd;
        JointCommand next_slave_cmd = slave_cmd;
        ret = master_controller->fcCalJointCommand(next_master_cmd);
        if (ret < 0) return ret;
        ret = slave_controller->fcCalJointCommand(next_slave_cmd);
        if (ret < 0) return ret;

        // next_master_cmd 和 next_slave_cmd 用于在同一控制周期
        // 分别向主、从机械臂下发关节位置、速度、加速度和力矩指令。
        // E_PLN_FINISHED 也需下发；下一周期返回 E_PLN_EXEC_EMPTY 才退出。
        master_cmd = next_master_cmd;
        slave_cmd = next_slave_cmd;
    }

    return E_PLN_OK;
}

若启用力控或柔顺控制，只调整两个 Controller 的控制模式和参数；两个阶段的 State → Task → ReferenceTrajectory → Controller → 同周期下发 顺序以及两套 JointCommand 的组织方式保持不变。

6. 常见问题

6.1 tpAddVelocityLine() 返回参数错误

先按 3.2 节检查空间、逆解策略和 6 维速度参数；再检查 Planner 是否同时包含 MASTER 和 SLAVE，以及主、从 State 的关节维数、TCP 和基座位姿是否正确。

6.2 tpAddPositionLine() 在某两个示教点之间失败

常见原因是协同直线中间位姿无解、连续逆解超关节限位或构型不连续。应在两点之间增加通过 speedL 获得的中间示教点，或重新选择夹持姿态；不应忽略错误后强行执行后续路段。

6.3 复现时未精确经过示教停留点

检查 4.2 节的交融半径设置；需精确经过示教点时应关闭交融。

6.4 主、从臂轨迹发送反了

按 3.3 节使用 TrajectoryPoint::id 路由，不要依赖输出 vector 的元素顺序。

6.5 更换 TCP 或夹持位置后出现拉扯

固定变换已与原 TCP 和夹持姿态不匹配。应停止任务，用当前实际关节角和新的主、从工具信息重新调用 rsInitiateRobotState() 并设置 IDEL，然后重新验证示教点。如果标定和安装误差无法充分消除，还应按 3.4 节增加合适的力控/柔顺控制，但力控不能代替固定变换的重新初始化。